#include <stdint-gcc.h>
#include <stddef.h>

#include "printf.h"
#include "uart.h"

#define MEMORY_DEPTH 0x10000000
#define PATTERN1 0x5a5a5a5a
#define PATTERN2 0xa5a5a5a5
#define W_OF_A(a) ((a)+1)

extern volatile uint32_t memory[MEMORY_DEPTH];

void myarm_und(uint32_t lr)
{
	uint32_t *p;
	puts("Undefined instruction\r");
	p = (uint32_t*)(lr-4);
	printf("%08x at %08x\r\n", *p, p);
	for (;;);
}

int test_data_bus(void)
{
	uint32_t w, r;
	for (w=1; w; w<<=1) {
		memory[0] = w;
		r = memory[0];
		if (r != w) {
			printf("a=0 w=%08x r=%08x\r\n", w, r);
			return -1;
		}
		memory[1] = w;
		r = memory[1];
		if (r != w) {
			printf("a=1 w=%08x r=%08x\r\n", w, r);
			return -1;
		}
	}
	return 0;
}

int test_address_bus(void)
{
	uint32_t a, r;
        uint32_t stuck_hi = 0, stuck_lo = 0;
	int err = 0;
	// check for lines stuck high
	for (a=1; a<MEMORY_DEPTH; a<<=1) {
		memory[a] = PATTERN1;
	}
	memory[0] = PATTERN2;
	for (a=1; a<MEMORY_DEPTH; a<<=1) {
		r = memory[a];
		if (r != PATTERN1) {
			printf("[%08x]=>%08x\r\n", a, r);
			if (r == PATTERN2) {
				stuck_hi |= a;
			}
			err = 1;
		}
	}
	// check for lines stuck low
	memory[0] = PATTERN1;
	for (a=1; a<MEMORY_DEPTH; a<<=1) {
		memory[a] = PATTERN2;
	}
	r = memory[0];
	if (r != PATTERN1) {
		printf("[0]=>%08x\r\n", r);
		if (r == PATTERN2) {
			stuck_lo |= a;
		}
		err = 1;
	}
#if 0
	for (a=1; a<MEMORY_DEPTH; a<<=1) {
		r = memory[a];
		if (r != PATTERN2) {
			printf("a=%08x w=%08x r=%08x\r\n", a, PATTERN2, r);
			return -1;
		}
	}
#endif
        if (err) {
		if (stuck_hi | stuck_lo) {
			printf("stuck_hi=%08x\r\nstuck_lo=%08x\r\n", stuck_hi, stuck_lo);
		}
		return -1;
        }
	return 0;
}

int test_device(void)
{
	uint32_t a, w, r;
	puts("Starting device test\r");
	puts("Pass 1 WRITE\r");
	a = 0;
	while (a < MEMORY_DEPTH) {
		w = W_OF_A(a);
		memory[a] = w;
		a++;
		if (!(a&0xfffff)) {
			printf("%uMB\r\n", a>>18);
		}
	}
	puts("Pass 1 READ\r");
	a = 0;
	while (a < MEMORY_DEPTH) {
		w = W_OF_A(a);
		r = memory[a];
		if (r != w) {
			printf("a=%08x w=%08x r=%08x\r\n", a, w, r);
			return -1;
		}
		a++;
		if (!(a&0xfffff)) {
			printf("%uMB\r\n", a>>18);
		}
	}
	puts("Pass 2 WRITE\r");
	a = 0;
	while (a < MEMORY_DEPTH) {
		w = ~W_OF_A(a);
		memory[a] = w;
		a++;
		if (!(a&0xfffff)) {
			printf("%uMB\r\n", a>>18);
		}
	}
	puts("Pass 2 READ\r");
	a = 0;
	while (a < MEMORY_DEPTH) {
		w = ~W_OF_A(a);
		r = memory[a];
		if (r != w) {
			printf("a=%08x w=%08x r=%08x\r\n", a, w, r);
			return -1;
		}
		a++;
		if (!(a&0xfffff)) {
			printf("%uMB\r\n", a>>18);
		}
	}
	return 0;
}

void main(void)
{
	//uart_init();
	puts("Memory test started\r");
	if (test_data_bus() == 0) {
		puts("Data bus OK\r");
		if (test_address_bus() == 0) {
			puts("Address bus OK\r");
			if (test_device() == 0) {
				puts("Device OK\r");
				puts("Everything OK\r");
			} else {
				puts("Device bad\r");
			}
		} else {
			puts("Address bus bad\r");
		}
	} else {
		puts("Data bus bad\r");
	}
}
